/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef _FLEET_FORMATION_GUI_
#define _FLEET_FORMATION_GUI_


#include <SDL/SDL.h>
#include <SDL/SDL_gfxPrimitives.h>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <map>
#include "ownutils.h"
#include "Component.hpp"
#include "gimi.h"
#include "sync.hpp"
#include "MaCICtrlClient.hpp"
#include "FleetFormationController.hpp"



struct SRobotIcon{
  SRobotIcon(float aPositionX,float aPositionY):
    positionX(aPositionX),
    positionY(aPositionY),
    iPositionClient(NULL),
    iSpeedCtrlClient(NULL){}
  

  /**
   *
   */
  void GetTriangleCorners(float &aX1,float &aY1,
                          float &aX2,float &aY2,
                          float &aX3,float &aY3){

   
    aX1 = positionX;
    aY1 = positionY -10;
    aX2 = positionX -10;
    aY2 = positionY +10;
    aX3 = positionX +10;
    aY3 = positionY +10;
   
  }
     
  //FIXME
  bool IsInsideRobot(float aX,float aY){

    if(aX >=positionX -10 &&aX <=positionX +10 &&
       aY >= positionY-10 &&aY <=positionY+10){
      return true;
    }else{
      return false;
    }

  }                    
  void SetPosition(float aX, float aY){
    positionX = aX;
    positionY = aY;


  }
  int positionX;
  int positionY;
  MaCI::Position::CPositionClient *iPositionClient;
  MaCI::SpeedCtrl::CSpeedCtrlClient *iSpeedCtrlClient;


};
struct SFleetGUIButton{

  SFleetGUIButton(int axMin,int ayMin,int axMax,int ayMax,std::string aText)
                  //            void *FunctionToCall):
    :xMin(axMin),
    yMin(ayMin),
    xMax(axMax),
    yMax(ayMax),
    text(aText){}
    //    iFunctionToCall(aFunctionToCall){}
    

  int xMin;
  int yMin;
  int xMax;
  int yMax;
  std::string text;
  //void (*iFunctionToCall)(void);
};
class CFleetFormationGUI: private gim::CSync,
                          public Gimbo::CComponent,
                          public MaCI::Position::CPositionClientCallback{


public:
  CFleetFormationGUI();
  ~CFleetFormationGUI();
  bool Initialize(gimi::GIMI *aGimiPtr,int aGimboID, int aReportID);
  void Deinitialize(void);
  
  void StartGUI();
  void StopGUI();

protected:
  //Gimbo component function
  void OnServiceAssignment(const Gimbo::TServiceAssignmentList &aServiceList);  

 //Position client callback
  void OnPositionDataEvent(const MaCI::Position::CPositionData &aData, 
                           const unsigned int aDataSequence,
                           MaCI::Position::CPositionClient &aPositionClient,
                           void * aUserPtr = NULL);
private:
  gimi::GIMI *iGimi;
  SDL_Surface *iSDLScreen;
  unsigned int iWidth;
  unsigned int iHeigth;
  bool iInitialized;
  bool iRunGUI;

  int iGridminX,iGridmaxX,iGridminY,iGridmaxY;
  Gimbo::TServiceAssignmentList iServiceList;
 
  
  bool InitializeSDL();
  void RunGUI();
  void CreateButtons();
  //Drawing functions
  void DrawScreen();
  void DrawGridAndScale();
  void DrawButtons();
  //Function to set robots to controller
  void SetRobotsToFleetFormationController();
  void DeleteMachines();
  std::map<std::string, SRobotIcon*> iRobotMap;
  std::map<std::string, SRobotIcon*>::iterator iMasterRobotIter;
  std::map<std::string, SRobotIcon*>::iterator iSelectedRobotIterator;
  std::vector<SFleetGUIButton *> iGUIButtons;
  int iPixelsToMeter;

  /// FLEET FORMATION CONTROLLER VARIABLES
  CFleetFormationController iFleetController;
  MaCI::Position::CPositionClient *iMasterPC;
  MaCI::SpeedCtrl::CSpeedCtrlClient *iMasterSC;
  std::map<std::string, MaCI::Position::CPositionClient *> iSlavePC;
  std::map<std::string, MaCI::SpeedCtrl::CSpeedCtrlClient *> iSlaveSC;
  

  // Drawing the real positions
  //  std::map<MaCI::Position::CPositionClient *, MaCI::Position::TPose2D> iRelativePositionsToMaster;

  // Private copy const.
  CFleetFormationGUI(const CFleetFormationGUI &) 
    : CSync(1,1),
      iGimi(NULL),
      iSDLScreen(NULL),
      iWidth(0),
      iHeigth(0),
      iInitialized(false),
      iRunGUI(false),
      iGridminX(0),
      iGridmaxX(0),
      iGridminY(0),
      iGridmaxY(0),
      iServiceList(),
      iRobotMap(),
      iMasterRobotIter(iRobotMap.end()),
      iSelectedRobotIterator(iRobotMap.end()),
      iGUIButtons(),
      iPixelsToMeter(0),
      iFleetController(),
      iMasterPC(NULL),
      iMasterSC(NULL),
      iSlavePC(),
      iSlaveSC(){}
    
    CFleetFormationGUI &operator=(const CFleetFormationGUI &) { return *this; }

};



#endif
